#include "stm32f10x.h"                  // Device header
#include "MyI2C.h"
#include "MPU6050_Reg.h"
#include "OLED.h"
#include "Delay.h"


#define MPU6050_ADDRESS		0xD0		//MPU6050的I2C从机地址

/**
  * 函    数：MPU6050写寄存器
  * 参    数：RegAddress 寄存器地址，范围：参考MPU6050手册的寄存器描述
  * 参    数：Data 要写入寄存器的数据，范围：0x00~0xFF
  * 返 回 值：0-成功，1-失败
  */
uint8_t MPU6050_WriteReg(uint8_t RegAddress, uint8_t Data)
{
    uint8_t ack;
    MyI2C_Start();
    ack = MyI2C_SendByte(MPU6050_ADDRESS);    // 发送从机地址
    if(ack != 0) goto error;                   // 无应答则出错
    
    ack = MyI2C_SendByte(RegAddress);         // 发送寄存器地址
    if(ack != 0) goto error;
    
    ack = MyI2C_SendByte(Data);               // 发送数据
    if(ack != 0) goto error;
    
    MyI2C_Stop();
    return 0; // 成功
    
error:
    MyI2C_Stop();
    return 1; // 失败
}

/**
  * 函    数：MPU6050读寄存器
  * 参    数：RegAddress 寄存器地址，范围：参考MPU6050手册的寄存器描述
  * 参    数：Data 读取到的数据指针
  * 返 回 值：0-成功，1-失败
  */
uint8_t MPU6050_ReadReg(uint8_t RegAddress, uint8_t *Data)
{
    uint8_t ack;
    
    MyI2C_Start();
    ack = MyI2C_SendByte(MPU6050_ADDRESS);    // 发送从机地址（写）
    if(ack != 0) goto error;
    
    ack = MyI2C_SendByte(RegAddress);         // 发送寄存器地址
    if(ack != 0) goto error;
    
    MyI2C_Start();                            // 重复起始条件
    ack = MyI2C_SendByte(MPU6050_ADDRESS | 0x01); // 发送从机地址（读）
    if(ack != 0) goto error;
    
    *Data = MyI2C_ReceiveByte();
    MyI2C_SendAck(1);                         // 发送非应答位
    MyI2C_Stop();
    return 0; // 成功
    
error:
    MyI2C_Stop();
    return 1; // 失败
}

/**
  * 函    数：MPU6050初始化
  * 参    数：无
  * 返 回 值：0-成功，1-失败
  */
// 在MPU6050.c中，暂时简化初始化流程
uint8_t MPU6050_Init(void)
{
    uint8_t id = 0;
    
    MyI2C_Init();  // 初始化I2C
    
    // 先不进行复杂的初始化，直接读取ID测试
    if(MPU6050_ReadReg(MPU6050_WHO_AM_I, &id) == 0) {
        if(id == 0x68) {
            OLED_Clear();
            OLED_ShowString(0,0,"MPU6050 Found!",OLED_8X16);
            OLED_Printf(0,16,OLED_8X16,"ID:0x%02X",id);
            OLED_Update();
            Delay_ms(1000);
            return 0;
        }
    }
    
    // 如果直接读取失败，尝试简单初始化
    MPU6050_WriteReg(MPU6050_PWR_MGMT_1, 0x00);  // 最简单的唤醒
    Delay_ms(100);
    
    if(MPU6050_ReadReg(MPU6050_WHO_AM_I, &id) == 0 && id == 0x68) {
        OLED_Clear();
        OLED_ShowString(0,0,"MPU6050 Woke Up!",OLED_8X16);
        OLED_Printf(0,16,OLED_8X16,"ID:0x%02X",id);
        OLED_Update();
        Delay_ms(1000);
        return 0;
    }
    
    // 完全失败
    OLED_Clear();
    OLED_ShowString(0,0,"MPU6050 Not Found",OLED_8X16);
    OLED_Printf(0,16,OLED_8X16,"ID:0x%02X",id);
    OLED_Update();
    Delay_ms(3000);
    return 1;
}

/**
  * 函    数：MPU6050获取ID号
  * 参    数：无
  * 返 回 值：MPU6050的ID号，0xFF表示读取失败
  */
uint8_t MPU6050_GetID(void)
{
    uint8_t id;
    if(MPU6050_ReadReg(MPU6050_WHO_AM_I, &id) == 0)
        return id;
    else
        return 0xFF; // 读取失败
}

/**
  * 函    数：MPU6050获取数据
  * 参    数：AccX AccY AccZ 加速度计X、Y、Z轴的数据，使用输出参数的形式返回，范围：-32768~32767
  * 参    数：GyroX GyroY GyroZ 陀螺仪X、Y、Z轴的数据，使用输出参数的形式返回，范围：-32768~32767
  * 返 回 值：0-成功，1-失败
  */
uint8_t MPU6050_GetData(int16_t *AccX, int16_t *AccY, int16_t *AccZ, 
						int16_t *GyroX, int16_t *GyroY, int16_t *GyroZ)
{
	uint8_t DataH, DataL;								//定义数据高8位和低8位的变量
	uint8_t result;
	
	// 读取加速度计X轴
	result = MPU6050_ReadReg(MPU6050_ACCEL_XOUT_H, &DataH);
	if(result != 0) return 1;
	result = MPU6050_ReadReg(MPU6050_ACCEL_XOUT_L, &DataL);
	if(result != 0) return 1;
	*AccX = (DataH << 8) | DataL;
	
	// 读取加速度计Y轴
	result = MPU6050_ReadReg(MPU6050_ACCEL_YOUT_H, &DataH);
	if(result != 0) return 1;
	result = MPU6050_ReadReg(MPU6050_ACCEL_YOUT_L, &DataL);
	if(result != 0) return 1;
	*AccY = (DataH << 8) | DataL;
	
	// 读取加速度计Z轴
	result = MPU6050_ReadReg(MPU6050_ACCEL_ZOUT_H, &DataH);
	if(result != 0) return 1;
	result = MPU6050_ReadReg(MPU6050_ACCEL_ZOUT_L, &DataL);
	if(result != 0) return 1;
	*AccZ = (DataH << 8) | DataL;
	
	// 读取陀螺仪X轴
	result = MPU6050_ReadReg(MPU6050_GYRO_XOUT_H, &DataH);
	if(result != 0) return 1;
	result = MPU6050_ReadReg(MPU6050_GYRO_XOUT_L, &DataL);
	if(result != 0) return 1;
	*GyroX = (DataH << 8) | DataL;
	
	// 读取陀螺仪Y轴
	result = MPU6050_ReadReg(MPU6050_GYRO_YOUT_H, &DataH);
	if(result != 0) return 1;
	result = MPU6050_ReadReg(MPU6050_GYRO_YOUT_L, &DataL);
	if(result != 0) return 1;
	*GyroY = (DataH << 8) | DataL;
	
	// 读取陀螺仪Z轴
	result = MPU6050_ReadReg(MPU6050_GYRO_ZOUT_H, &DataH);
	if(result != 0) return 1;
	result = MPU6050_ReadReg(MPU6050_GYRO_ZOUT_L, &DataL);
	if(result != 0) return 1;
	*GyroZ = (DataH << 8) | DataL;
	
	return 0; // 成功
}
